#include <chrono>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

namespace multi_launch_demo
{

class ComponentProcessor : public rclcpp::Node
{
   public:
    ComponentProcessor(const rclcpp::NodeOptions &options) : Node("component_processor", options)
    {
        // 声明参数
        this->declare_parameter("processing_delay_ms", 100);
        this->declare_parameter("add_timestamp", true);

        // 获取参数
        processing_delay_ms_ = this->get_parameter("processing_delay_ms").as_int();
        add_timestamp_ = this->get_parameter("add_timestamp").as_bool();

        // 创建订阅者 - 接收原始数据
        input_subscription_ = this->create_subscription<std_msgs::msg::String>(
            "raw_data", 10, [this](const std_msgs::msg::String::SharedPtr msg) { this->process_data(msg); });

        // 创建发布者 - 发布处理后的数据
        output_publisher_ = this->create_publisher<std_msgs::msg::String>("processed_data", 10);

        // 创建定时器 - 定期发布处理统计
        stats_timer_ = this->create_wall_timer(5s, [this]() -> void { this->publish_stats(); });

        RCLCPP_INFO(this->get_logger(), "Component Processor started - Delay: %dms, Timestamp: %s",
                    processing_delay_ms_, add_timestamp_ ? "enabled" : "disabled");
    }

   private:
    void process_data(const std_msgs::msg::String::SharedPtr input_msg)
    {
        // 模拟数据处理延迟
        if (processing_delay_ms_ > 0)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(processing_delay_ms_));
        }

        // 处理数据
        auto output_msg = std_msgs::msg::String();
        output_msg.data = "[PROCESSED] " + input_msg->data;

        // 添加时间戳
        if (add_timestamp_)
        {
            auto now = this->get_clock()->now();
            output_msg.data += " @ " + std::to_string(now.nanoseconds());
        }

        // 发布处理后的数据
        output_publisher_->publish(output_msg);

        // 更新统计
        processed_count_++;

        RCLCPP_DEBUG(this->get_logger(), "Processed message %d: '%s' -> '%s'", processed_count_,
                     input_msg->data.c_str(), output_msg.data.c_str());
    }

    void publish_stats()
    {
        auto stats_msg = std_msgs::msg::String();
        stats_msg.data = "Processor Stats: " + std::to_string(processed_count_) + " messages processed";

        // 发布统计信息到统计主题
        if (stats_publisher_)
        {
            stats_publisher_->publish(stats_msg);
        }

        RCLCPP_INFO(this->get_logger(), "Published stats: %s", stats_msg.data.c_str());
    }

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr input_subscription_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr output_publisher_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr stats_publisher_;
    rclcpp::TimerBase::SharedPtr stats_timer_;

    int processing_delay_ms_;
    bool add_timestamp_;
    int processed_count_ = 0;
};

}  // namespace multi_launch_demo

RCLCPP_COMPONENTS_REGISTER_NODE(multi_launch_demo::ComponentProcessor)
